/*
 * AnalyzedImage.cpp
 *
 *  Created on: 10/09/2011
 *      Author: solnox
 */

#include "AnalyzedImage.h"
#include "ImageAnalyzer.h"
using namespace cv;

AnalyzedImage::AnalyzedImage() {
}

AnalyzedImage::AnalyzedImage(cv::Mat * image_ptr) {
    _is_pose_set = false;
    if(image_ptr != NULL)
    {
        rgb_image = *image_ptr;
        cvtColor(rgb_image, bw_image, CV_RGB2GRAY);
        float pp_x = ((float)bw_image.cols)/2.0f;
        float pp_y = ((float)bw_image.rows)/2.0f;
        _principal_point = Point2f(pp_x,pp_y);

        ImageAnalyzer::getInstance().detectKeyPoints(bw_image, keypoints) ;
        ImageAnalyzer::getInstance().computeDescriptors(bw_image,keypoints,desc);
        kd_tree  = new ::KDTree(desc);
    }
}

AnalyzedImage::~AnalyzedImage() {
}

void AnalyzedImage::setPose(cv::Mat  K,cv::Mat  R, cv::Mat T)
{
    Mat RT = Mat::zeros(3,4,CV_64FC1);
    Mat roi = RT(Range(0,3),Range(0,3));
    R.copyTo(roi);
    roi = RT(Range(0,3),Range(3,4));
    T.copyTo(roi);
    _P = K*RT;
    _K = K.clone();
    _R = R.clone();
    _T = T.clone();
    _is_pose_set = true;
}

void AnalyzedImage::setPose(Parameters* params)
{
    Mat K = params->getK();
    Mat R = params->getR();
    Mat T = params->getT();
    setPose(K,R,T);
}

cv::Mat AnalyzedImage::getModelT()
{
    //Mat M = _K * _R;
    Mat model_T = -_R.t()*_T;
    return model_T;
}
